#include "Bezier.hpp"

Bezier::Bezier(const std::vector<double>& xData, const std::vector<double>& yData,
               const std::vector<double>& PxData, const std::vector<double>& PyData) {
    int m = xData.size();
    controlPoints.resize(m, std::vector<std::vector<double>>(4, std::vector<double>(2)));
    
    for (int i = 0; i < m; ++i) {
        controlPoints[i][0][0] = xData[i];
        controlPoints[i][1][0] = xData[i] + 1.0/3.0 * PxData[i]; 
        controlPoints[i][2][0] = xData[(i+1)%m] - 1.0/3.0 * PxData[(i+1)%m];
        controlPoints[i][3][0] = xData[(i+1)%m];
        controlPoints[i][0][1] = yData[i];
        controlPoints[i][1][1] = yData[i] + 1.0/3.0 * PyData[i]; 
        controlPoints[i][2][1] = yData[(i+1)%m] - 1.0/3.0 * PyData[(i+1)%m];
        controlPoints[i][3][1] = yData[(i+1)%m];
    }
}

std::vector<double> Bezier::operator()(double t) const {
    int m = controlPoints.size();
    int segment = std::floor(t);
    t -= segment;
    segment = segment % m;

    double x = 0, y = 0;
    for (int i = 0; i < 4; ++i) {
        x += controlPoints[segment][i][0] * BernsteinPolynomial(3, i, t);
        y += controlPoints[segment][i][1] * BernsteinPolynomial(3, i, t);
    }
    return {x, y}; 
}

unsigned long long binom(unsigned int n, unsigned int k) {
    if (k > n - k) {
        k = n - k;
    }
    unsigned long long result = 1;
    for (unsigned int i = 0; i < k; ++i) {
        result *= (n - i);
        result /= (i + 1);
    }
    return result;
}

double Bezier::BernsteinPolynomial(int n, int k, double t) {
    return std::pow(t, k) * std::pow(1 - t, n - k) * binom(n, k);
}
